#!/usr/bin/env python3
import os
import sys
import time
import gpiod
import logging
import netifaces
import importlib
import threading
import subprocess
import socketserver
import re
from find_device import get_cpu_serial_number
import ros_robot_controller_sdk as rrc


led1_pin = 16
try:
    chip = gpiod.Chip('gpiochip4')
    led1 = chip.get_line(led1_pin)
    led1.request(consumer="led1", type=gpiod.LINE_REQ_DIR_OUT)
except Exception as e:
    print(f"GPIO初始化失败: {e}。将使用模拟LED进行逻辑演示。")
    # 创建一个虚拟led对象，以便在没有硬件的PC上测试逻辑
    class MockLed:
        def set_value(self, value):
            state = "亮" if value == 0 else "灭"
            print(f"\r[模拟LED状态]: {state}   ", end='', flush=True)
    led1 = MockLed()

led_on_time = 0   # LED亮的时间 (单位: 10毫秒)
led_off_time = 0  # LED灭的时间 (单位: 10毫秒) 



# 获取当前脚本所在路径
path = os.path.split(os.path.realpath(__file__))[0]
log_file_path = os.path.join(path, "wifi.log")

# 创建日志文件（如果不存在）
if not os.path.exists(log_file_path):
    os.system(f'touch {log_file_path}')

# 配置文件路径
config_file_name = "wifi_conf.py"
internal_config_file_dir_path = "/etc/wifi"
external_config_file_dir_path = path
internal_config_file_path = os.path.join(internal_config_file_dir_path, config_file_name)
external_config_file_path = os.path.join(external_config_file_dir_path, config_file_name)
network_file = "/home/pi/docker/tmp/.networkmode.txt"

# 全局变量
ros_control_enabled = False
current_wifi_mode = None  # 用于记录当前的网络模式
server = None  # 全局服务器实例
ip = None  # 当前IP地址

# 初始化Board实例
# try:
#     board = rrc.Board()
# except Exception as e:
#     board = None
# 配置日志记录
logger = logging.getLogger("WiFi工具")
logger.setLevel(logging.DEBUG)
log_handler = logging.FileHandler(log_file_path)
log_handler.setLevel(logging.INFO)
log_formatter = logging.Formatter('%(name)s - %(asctime)s - %(levelname)s - %(message)s')
log_handler.setFormatter(log_formatter)
logger.addHandler(log_handler)


def led_thread():
    """
    该线程独立运行，只根据全局变量 led_on_time 和 led_off_time 的值来控制LED。
    """
    global led_on_time, led_off_time
    
    local_led_on_time = -1 # 强制第一次更新
    local_led_off_time = -1

    count = 0
    cycle_time = 0
    while True:
        # 当主线程修改了全局变量后，在这里更新本地变量
        if local_led_on_time != led_on_time or local_led_off_time != led_off_time:
            local_led_on_time = led_on_time
            local_led_off_time = led_off_time
            cycle_time = local_led_on_time + local_led_off_time
            count = 0
        
        if cycle_time == 0:
            led1.set_value(0 if local_led_on_time > 0 else 1)
            time.sleep(0.1)
            continue

        if count < local_led_on_time:
            led1.set_value(0) # 亮
        else:
            led1.set_value(1) # 灭
        
        count = (count + 1) % cycle_time
        time.sleep(0.01)


##### MACHINE TYEP SET ######
def set_machine_ros():
    ros_machine_command = "ros2 service call /ros_robot_controller/set_machine_type std_srvs/srv/Trigger '{}'"
    full_command = [
        'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
        '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_machine_command}"
    ]
    result = subprocess.run(full_command, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)

def get_machine_type_from_config(config_path):
    try:
        with open(config_path, 'r') as f:
            for line in f:
                line = line.strip()
                match = re.match(r'^\s*export\s+MACHINE_TYPE\s*=\s*["\']?(.*?)["\']?\s*$', line)
                if match:
                    return match.group(1)
            raise ValueError(f"MACHINE_TYPE IS None")
    except FileNotFoundError:
        raise FileNotFoundError(f"CONFIG PATH IS ERROR")

def get_factory_mode_from_config(config_path):
    try:
        with open(config_path, 'r') as f:
            for line in f:
                line = line.strip()
                match = re.match(r'^\s*export\s+FactoryMode\s*=\s*["\']?(.*?)["\']?\s*$', line)
                if match:
                    return match.group(1) # 返回匹配到的值 (online/offline)
        return "offline"
    except FileNotFoundError:
        return "offline" # 文件不存在时返回一个安全的默认值

# def send_machine_type(board,machine_type):
#     if 'Tank' in machine_type:
#         motor_type = 0x01
#         battery_level = 0x1af4
#     elif 'Acker' in machine_type or 'Mecanum' in machine_type:
#         motor_type = 0x02
#         battery_level = 0x1af4
#     else:
#         motor_type = 0x09
#         battery_level = 0x1af4
#     if motor_type is not None:
#         for i in range(2):
#             '''
#             MOTOR_TYPE_JGB520 0x00
#             MOTOR_TYPE_JGB37  0x01
#             MOTOR_TYPE_JGB27  0x02
#             MOTOR_TYPE_JGB528 0x03
#             '''
#             board.set_motor_type(motor_type)
#             board.set_battery_level(battery_level)
#     else:
#         print('Please Set the machine_type')

# def set_machine_serial(board):
#     config_path = "/home/pi/docker/tmp/.typerc"
#     machine_type = get_machine_type_from_config(config_path)
#     send_machine_type(board,machine_type)

###################################


def update_globals(module):
    """
    动态导入或重新加载模块，并将模块中的变量更新到全局命名空间。
    """
    if module in sys.modules:
        mdl = importlib.reload(sys.modules[module])
    else:
        mdl = importlib.import_module(module)
    if "__all__" in mdl.__dict__:
        names = mdl.__dict__["__all__"]
    else:
        names = [x for x in mdl.__dict__ if not x.startswith("_")]
    globals().update({k: getattr(mdl, k) for k in names})

def get_wifi_ip():
    """
    获取当前连接的WiFi接口的IP地址。
    """
    interfaces = netifaces.interfaces()

    for interface in interfaces:
        if interface.startswith('wlan'):
            addresses = netifaces.ifaddresses(interface)
            if netifaces.AF_INET in addresses:
                ip_info = addresses[netifaces.AF_INET][0]
                ip_address = ip_info['addr']
                return ip_address

    return None

def reset_wifi():
    """
    重置WiFi配置并重启WiFi服务。
    """
    os.system("sudo rm /etc/wifi/* -rf > /dev/null 2>&1")
    os.system("sudo systemctl restart wifi.service > /dev/null 2>&1")
    logger.info("WiFi已重置并重启服务。")

class TCPHandler(socketserver.BaseRequestHandler):
    """
    TCP请求处理类，用于接收命令并执行相应操作。
    """
    def handle(self):
        self.request.settimeout(2)
        data = b""
        while True:
            try:
                chunk = self.request.recv(1024)
                if not chunk:
                    break
                data += chunk
                if data.strip() == b"resetwifi":
                    logger.info("收到重置WiFi命令。")
                    reset_wifi()
                    break
            except socket.timeout:
                logger.warning('TCP连接超时。')
                break
            except Exception as e:
                logger.error(f"TCP处理时出错: {e}")
                break
        self.request.close()

class PhoneServer(socketserver.TCPServer):
    """
    定制的TCP服务器类。
    """
    def __init__(self, server_address, RequestHandlerClass):
        super().__init__(server_address, RequestHandlerClass)

    def handle_timeout(self):
        logger.warning('TCP服务器超时。')

def start_server(ip, port):
    """
    启动TCP服务器。
    """
    server = PhoneServer((ip, port), TCPHandler)
    thread = threading.Thread(target=server.serve_forever)
    thread.daemon = True
    thread.start()
    logger.info(f"TCP服务器已启动在 {ip}:{port}")
    return server

def stop_server(server):
    """
    停止TCP服务器。
    """
    if server:
        server.shutdown()
        server.server_close()
        logger.info("TCP服务器已停止。")
    return None

def check_ros2_control():
    """
    检查ROS2控制节点是否存在，以决定LED控制方式。
    """
    global ros_control_enabled
    try:
        process = subprocess.Popen(
            ["docker", "exec", '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi', 
             '/bin/zsh', '-c', 'source /home/ubuntu/.zshrc && ros2 topic list'],
            stdout=subprocess.PIPE,
            stderr=subprocess.PIPE
        )
        stdout, stderr = process.communicate(timeout=5)  # 设置超时为5秒
        topics = stdout.decode().split('\n')
        if '/ros_robot_controller/set_led' in topics:
            ros_control_enabled = True
            logger.info("检测到ROS控制节点，启用ROS控制LED。")
        else:
            ros_control_enabled = False
            logger.info("未检测到ROS控制节点，使用本地控制LED。")
    except subprocess.TimeoutExpired:
        process.kill()
        ros_control_enabled = False
        logger.warning("检测ROS2控制超时，使用本地控制LED。")
    except Exception as e:
        ros_control_enabled = False
        logger.error(f"检测ROS2节点时出错: {e}")

# def set_led(led_id, on_time, off_time, repeat=0):
#     """
#     设置LED状态，根据ROS控制是否启用，选择不同的控制方式。
#     """
#     if ros_control_enabled:
#         ros_command = f"ros2 topic pub /ros_robot_controller/set_led ros_robot_controller_msgs/msg/LedState '{{id: {led_id}, on_time: {on_time}, off_time: {off_time}, repeat: {repeat}}}' --once"
#         full_command = [
#             'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
#             '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_command}"
#         ]
#         logger.info(f"执行ROS命令: {' '.join(full_command)}")
#         result = subprocess.run(full_command, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
#         set_machine_ros()
#         if result.returncode != 0:
#             logger.error(f"发布ROS 2消息时出错: {result.stderr.decode('utf-8')}")
#     else:
#         try:
#             board.set_led(on_time, off_time, repeat=repeat, led_id=led_id)
#             logger.info(f"使用rrc.Board本地控制LED: id={led_id}, on_time={on_time}, off_time={off_time}, repeat={repeat}")
#             set_machine_serial(board) 
#         except Exception as e:
#             logger.error(f"本地控制LED时出错: {e}")

def get_connect():
    """
    获取当前连接的WiFi名称。
    """
    try:
        result = subprocess.run(['nmcli', '-t', 'con', 'show', '--active'], stdout=subprocess.PIPE)
        active_conns = result.stdout.decode().split('\n')
        for conn in active_conns:
            if conn:
                conn_details = conn.split(':')
                if len(conn_details) >= 3 and 'wireless' in conn_details[2]:
                    wifi = conn_details[0]
                    return wifi
    except Exception as e:
        logger.error(f"获取WiFi连接时出错: {e}")
    return None

def disconnect():
    """
    断开当前WiFi连接。
    """
    try:
        wifi = get_connect()
        if wifi:
            os.system(f'nmcli connection down {wifi}')
            os.system(f'nmcli connection delete {wifi}')
            os.system('rm /etc/NetworkManager/system-connections/*')
            logger.info(f"已断开并删除WiFi连接: {wifi}")
    except Exception as e:
        logger.error(f"断开WiFi连接时出错: {e}")

def setup_ap_mode():
    """
    切换到AP模式的具体实现。
    """
    global current_wifi_mode, ip, server
    global led_on_time, led_off_time
    with open(network_file, 'w') as f:
       f.write('AP')

    current_wifi_mode = 1
    # set_led(1, 1, 0, 1)  # LED1常亮
    # set_led(2, 0.5, 0.5, 0)  # LED2闪烁
    logger.info("设置LED为AP模式: 均匀闪烁")
    led_on_time = 50
    led_off_time = 50

    disconnect()

    # 配置AP模式
    try:
        os.system(f'nmcli con add type wifi ifname wlan0 con-name {WIFI_AP_SSID} autoconnect yes ssid {WIFI_AP_SSID}')
        os.system(f'nmcli con modify {WIFI_AP_SSID} 802-11-wireless.mode ap ipv4.method shared')
        os.system(f'nmcli con modify {WIFI_AP_SSID} wifi-sec.key-mgmt wpa-psk wifi-sec.psk {WIFI_AP_PASSWORD}')
        os.system(f'nmcli con modify {WIFI_AP_SSID} wifi.band {WIFI_FREQ_BAND} wifi.channel {WIFI_CHANNEL}')
        os.system(f'nmcli con modify {WIFI_AP_SSID} ipv4.addresses {WIFI_AP_GATEWAY}/24')
        logger.info(f"配置AP模式: SSID={WIFI_AP_SSID}, 密码={WIFI_AP_PASSWORD}, 频段={WIFI_FREQ_BAND}, 频道={WIFI_CHANNEL}")

        # 等待AP模式生效
        timeout = 0
        while True:
            timeout += 1
            wifi = get_connect()
            if wifi == WIFI_AP_SSID:
                logger.info(f"AP创建成功: {WIFI_AP_SSID}")
                ip = WIFI_AP_GATEWAY
                server = start_server(ip, 9028)
                break
            if timeout == 20:
                logger.warning("重启NetworkManager...")
                os.system('systemctl restart NetworkManager')
            if timeout > 20:
                logger.error("无法创建AP，重启系统...")
                os.system('reboot')
                break
            time.sleep(1)
    except Exception as e:
        logger.error(f"设置AP模式时出错: {e}")

def setup_client_mode():
    """
    切换到客户端模式的具体实现。
    """
    global current_wifi_mode, ip, server
    global led_on_time, led_off_time

    with open(network_file, 'w') as f:
       f.write('STA')

    current_wifi_mode = 2
    # set_led(1, 1, 0, 1)  # LED1常亮
    # set_led(2, 0.05, 0.05, 0)  # LED2 50ms闪烁
    logger.info("设置LED为Client模式(正在连接): 快速闪烁")
    led_on_time = 5
    led_off_time = 5    

    disconnect()

    retry_count = 0
    max_retries = 3

    # 尝试连接客户端模式三次
    while retry_count < max_retries:
        try:
            p = subprocess.Popen(['nmcli', 'device', 'wifi', 'connect', WIFI_STA_SSID, 'password', WIFI_STA_PASSWORD], 
                                 stdout=subprocess.PIPE, stderr=subprocess.PIPE)
            stdout, stderr = p.communicate(timeout=10)  # 设置超时为10秒
            if p.returncode == 0:
                logger.info(f"成功连接到WiFi: {WIFI_STA_SSID}")
                break
            else:
                retry_count += 1
                logger.warning(f"连接到WiFi失败: {stderr.decode('utf-8')}. 重试 {retry_count}/{max_retries}")
                time.sleep(2)
        except subprocess.TimeoutExpired:
            p.kill()
            retry_count += 1
            logger.warning(f"连接到WiFi超时。重试 {retry_count}/{max_retries}")
        except Exception as e:
            p.kill()
            retry_count += 1
            logger.error(f"连接到WiFi时出错: {e}. 重试 {retry_count}/{max_retries}")

    if retry_count >= max_retries:
        logger.error(f"无法连接到SSID: {WIFI_STA_SSID}，切换到AP模式。")
        # 删除 /etc/wifi/* 文件
        try:
            os.system("sudo rm /etc/wifi/* -rf > /dev/null 2>&1")
            logger.info("已删除 /etc/wifi/* 文件。")
        except Exception as e:
            logger.error(f"删除 /etc/wifi/* 文件时出错: {e}")
        # 切换到AP模式
        setup_ap_mode()
        return

    # 检查是否成功连接并获取IP
    timeout = 0
    while True:
        ssid_name = get_connect()
        if ssid_name == WIFI_STA_SSID:
            # logger.info(f"已连接到 {WIFI_STA_SSID}")
            # set_led(1, 1, 0, 1)  # LED1常亮
            # set_led(2, 1, 0, 1)  # LED2常亮，表示成功连接
            logger.info("设置LED为Client模式(连接成功): 常亮")
            led_on_time = 100
            led_off_time = 0

            # 获取IP地址
            while True:
                wifi_ip = get_wifi_ip()
                if wifi_ip:
                    if wifi_ip != ip:
                        ip = wifi_ip
                        if server:
                            server = stop_server(server)
                        server = start_server(ip, 9028)
                    break
                time.sleep(1)
            break
        else:
            if timeout >= WIFI_TIMEOUT:
                logger.error(f"无法连接到SSID: {WIFI_STA_SSID}，切换到AP模式。")
                # 删除 /etc/wifi/* 文件
                try:
                    os.system("sudo rm /etc/wifi/* -rf > /dev/null 2>&1")
                    logger.info("已删除 /etc/wifi/* 文件。")
                except Exception as e:
                    logger.error(f"删除 /etc/wifi/* 文件时出错: {e}")
                # 切换到AP模式
                setup_ap_mode()
                break
            timeout += 1
            time.sleep(1)

def WIFI_MGR():
    """
    管理WiFi连接的主逻辑，根据WIFI_MODE进行模式切换。
    """
    global WIFI_AP_SSID, WIFI_STA_SSID, WIFI_AP_PASSWORD, WIFI_STA_PASSWORD
    global server, ip, current_wifi_mode
    global led_on_time, led_off_time

    check_ros2_control()  # 在WiFi管理器启动时检查ROS控制

    if WIFI_MODE == 1:  # AP模式
        setup_ap_mode()
    elif WIFI_MODE == 2:  # 客户端模式
        setup_client_mode()
    else:
        logger.error("无效的WIFI_MODE")
        # set_led(1, 1, 0, 1)  # LED1常亮
        # set_led(2, 0.5, 0.5, 0)  # LED2闪烁，表示错误
        led_on_time = 50
        led_off_time = 50


if __name__ == "__main__":

    typerc_config_path = "/home/pi/docker/tmp/.typerc"
    factory_mode = get_factory_mode_from_config(typerc_config_path)
    logger.info(f"从配置文件获取的 FactoryMode 为: {factory_mode}")
    # 配置参数
    if factory_mode == 'online':
        ap_prefix = 'HW-'
    else:
        ap_prefix = 'WN-'
    sn = get_cpu_serial_number()   # 获取CPU序列号
    WIFI_MODE = 2  # 1表示AP模式，2表示客户端模式，3表示AP模式并共享eth0的互联网
    WIFI_AP_SSID = ''.join([ap_prefix, sn[0:8]])
    WIFI_STA_SSID = "ssid"  # 请替换为目标WiFi名称
    WIFI_AP_PASSWORD = "hiwonder"  # 请替换为AP模式的密码
    WIFI_STA_PASSWORD = "12345678"  # 请替换为客户端模式的密码
    WIFI_AP_GATEWAY = "192.168.149.1"
    WIFI_CHANNEL = 36
    WIFI_FREQ_BAND = 'a'  # 'a'表示5G, 'g'表示2.4G
    WIFI_TIMEOUT = 30  # 客户端模式下的连接超时时间（秒）
    # WIFI_LED = True
    ip = WIFI_AP_GATEWAY

    # 读取配置文件
    if os.path.exists(config_file_name):
        update_globals(os.path.splitext(config_file_name)[0])
    if os.path.exists(internal_config_file_path):
        sys.path.insert(0, internal_config_file_dir_path)
        update_globals(os.path.splitext(config_file_name)[0])
    if os.path.exists(external_config_file_path):
        sys.path.insert(1, external_config_file_dir_path)
        update_globals(os.path.splitext(config_file_name)[0])

    # 启动WiFi管理线程
    # if WIFI_LED:
        # wifi_thread = threading.Thread(target=WIFI_MGR)
        # wifi_thread.start()
    # 启动LED控制线程
    led_controller_thread = threading.Thread(target=led_thread)
    led_controller_thread.start()

    # 启动WiFi管理线程
    wifi_thread = threading.Thread(target=WIFI_MGR)
    wifi_thread.start()
    wifi_thread.join()        

    # # 启动ROS节点监控线程
    # node_monitor_thread = threading.Thread(target=monitor_ros_node)
    # node_monitor_thread.start()


